#include "ManFifo.h"
#include <stdlib.h>
#include <string.h>

ManFifo::ManFifo(uint16_t   bufferSize, uint16_t   packLenBufferSize)
{
	this->m_bufferSize = bufferSize;
	this->m_buffer = new uint8_t[bufferSize];
	this->m_packLenBuffer = new uint8_t[packLenBufferSize];
	this->m_packLenBufferSize = packLenBufferSize;
	this->lock = NULL;
	this->unlock = NULL;
	Clean();
}

ManFifo::~ManFifo()
{
	delete[] m_buffer;
	delete[] m_packLenBuffer;
}
void ManFifo::Clean(void) {
	this->m_occupyPackCount = 0;
	this->m_iPop = 0;
	this->m_iPush = 0;
	this->m_iPackLenPop = 0;
	this->m_iPackLenPush = 0;

}


uint16_t ManFifo::Add(void* buf, uint8_t len)
{
	if (this->lock != NULL) {
		this->lock();
	}
	if (this->m_occupyPackCount >= this->m_packLenBufferSize)
	{
		if (this->unlock != NULL) {
			this->unlock();
		}
		return 0;
	}
	uint8_t* p = (uint8_t*)buf;
	this->m_packLenBuffer[this->m_iPackLenPush] = len;
	if (++this->m_iPackLenPush >= this->m_packLenBufferSize) this->m_iPackLenPush = 0;
	uint8_t i;
	if (this->m_iPush + len < this->m_bufferSize)
	{
		for (i = 0; i < len; ++i)
		{
			this->m_buffer[this->m_iPush] = *p;
			++this->m_iPush;
			++p;
		}
	}
	else
	{
		uint8_t size = this->m_bufferSize - this->m_iPush;
		for (i = 0; i < size; ++i)
		{
			this->m_buffer[this->m_iPush] = *p;
			++this->m_iPush;
			++p;
		}
		this->m_iPush = 0;
		for (i = size; i < len; ++i)
		{
			this->m_buffer[this->m_iPush] = *p;
			++this->m_iPush;
			++p;
		}
	}
	this->m_occupyPackCount = this->m_occupyPackCount + 1;
	if (this->unlock != NULL) {
		this->unlock();
	}
	return len;
}



uint16_t ManFifo::Read(void* rBuf, uint8_t maxLen)
{
	uint16_t   retLen = 0;
	if (this->m_occupyPackCount == 0) {
		return retLen;
	}
	if (this->lock != NULL) {
		this->lock();
	}
	if (this->m_occupyPackCount == 0)
	{
		if (this->unlock != NULL) {
			this->unlock();
		}
		return retLen;
	}
	uint8_t* p = (uint8_t*)rBuf;
	retLen = this->m_packLenBuffer[this->m_iPackLenPop];
	if (++this->m_iPackLenPop >= this->m_packLenBufferSize) this->m_iPackLenPop = 0;
	uint8_t i;

	if (this->m_iPop + retLen < this->m_bufferSize)
	{
		for (i = 0; i < retLen; ++i)
		{
			*p = this->m_buffer[this->m_iPop];
			++this->m_iPop;
			++p;
		}
	}
	else
	{
		uint8_t size = this->m_bufferSize - this->m_iPop;
		for (i = 0; i < size; ++i)
		{
			*p = this->m_buffer[this->m_iPop];
			++this->m_iPop;
			++p;
		}
		this->m_iPop = 0;
		for (i = size; i < retLen; ++i)
		{
			*p = this->m_buffer[this->m_iPop];
			++this->m_iPop;
			++p;
		}
	}
	*p = 0;
	this->m_occupyPackCount = this->m_occupyPackCount - 1;
	if (this->unlock != NULL) {
		this->unlock();
	}
	return retLen;
}




ManFifoBuffer::ManFifoBuffer(uint16_t   bBufferSize) {
	m_bBuffer = new uint8_t[bBufferSize];;
	m_capacity = bBufferSize;
	m_iPush = 0;
	m_iPop = 0;
}

ManFifoBuffer::~ManFifoBuffer() {

	delete[] m_bBuffer;
}

uint32_t ManFifoBuffer::Write(uint8_t* pbuf, uint32_t size) {
	uint32_t w_size = 0, free_size = 0;

	if ((size == 0) || (pbuf == NULL))
	{
		return 0;
	}

	free_size = FreeSize();
	if (free_size == 0)
	{
		return 0;
	}

	if (free_size < size)
	{
		size = free_size;
	}
	w_size = size;
	while (w_size-- > 0)
	{
		m_bBuffer[m_iPush++] = *pbuf++;
		if (m_iPush >= m_capacity)
		{
			m_iPush = 0;
		}
	}
	return size;
}

uint32_t ManFifoBuffer::Read(uint8_t* pbuf, uint32_t size)
{
	uint32_t r_size = 0, occupy_size = 0;

	if ((size == 0) || (pbuf == NULL))
	{
		return 0;
	}

	occupy_size = OccupySize();
	if (occupy_size == 0)
	{
		return 0;
	}

	if (occupy_size < size)
	{
		size = occupy_size;
	}
	r_size = size;
	while (r_size-- > 0)
	{
		*pbuf++ = m_bBuffer[m_iPop++];
		if (m_iPop >= m_capacity)
		{
			m_iPop = 0;
		}
		occupy_size--;
	}
	return size;
}


void ManFifoBuffer::Reset()
{
	m_iPush = 0;
	m_iPop = 0;
}


uint32_t ManFifoBuffer::TotalSize()
{

	return m_capacity;
}

uint32_t ManFifoBuffer::FreeSize()
{
	uint32_t size;
	size = m_capacity - OccupySize() - 1;
	return size;
}


uint32_t ManFifoBuffer::OccupySize()
{
	if (m_iPush == m_iPop) {
		return 0;
	}
	if (m_iPush > m_iPop) {
		return m_iPush - m_iPop;
	}
	else {
		return m_iPush + m_capacity - m_iPop;
	}
}






ManFifoWrap::ManFifoWrap() {

	m_manFifo = new ManFifo(512, 5);
}

ManFifoWrap::~ManFifoWrap() {
	delete m_manFifo;
}

uint16_t ManFifoWrap::Count() {
	return m_manFifo->Count();
}



uint32_t ManFifoWrap::Write(uint8_t* pbuf, uint32_t size) {

	if (m_cache_index + size > 256) {
		return 0;
	}
	memcpy(&m_cache[m_cache_index], pbuf, size);
	m_cache_index = m_cache_index + size;
	m_cache_timeOut = 0;
	return 0;
}




uint32_t ManFifoWrap::Read(uint8_t* pbuf, uint32_t size) {

	return m_manFifo->Read(pbuf, size);
}

void ManFifoWrap::OnTick(uint32_t ms) {
	m_cache_timeOut++;
	if (m_cache_timeOut++ > 2 && m_cache_index > 0) {
		m_cache_timeOut = 0;
		m_manFifo->Add(m_cache, m_cache_index);
		m_cache_index = 0;
	}

}